//
// Created by k on 25-6-24.
//

//
// Created by k on 25-6-24.
//

#include <angles/angles.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <ros/package.h>
#include <ros/ros.h>
#include <sensor_msgs/BatteryState.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Int8.h>
#include <std_msgs/String.h>
#include <std_srvs/SetBool.h>
#include <tf/transform_broadcaster.h>
#include "chassis.h"

#define CMD_TIME_OUT 300  // ms

static float aim_x_vel = 0.0;
static float aim_y_vel = 0.0;
static float aim_z_omega = 0.0;
static uint32_t light = 0;
uint32_t io = 0;
uint32_t sys_cmd = 0;
static ros::Time cmd_update_time;

void cmdMsgCallback(const geometry_msgs::Twist &cmd) {
    ROS_INFO_THROTTLE(2.0, "recv cmd:%f, %f", cmd.linear.x, cmd.angular.z);
    aim_x_vel = cmd.linear.x;
    aim_y_vel = cmd.linear.y;
    aim_z_omega = cmd.angular.z;
    cmd_update_time = ros::Time::now();
}

void motorCmdCallback(const std_msgs::Bool &msg) {
    msg.data ? sys_cmd |= 1 : sys_cmd &= ~(1);
    setSysCmd(sys_cmd);
}

void ioCallback(const std_msgs::Int32 &msg) {
    msg.data < 0 ? io |= (1 << (-msg.data - 1)) : io &= ~(1 << (msg.data - 1));
    setIo(io);
}

void stateLightCallback(const std_msgs::Int8 &value) { light = value.data; }
void infoCallback(const std_msgs::String &info) { setText(info.data.c_str()); }

bool charge(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res) {
    req.data ? sys_cmd |= 2 : sys_cmd &= ~(2);
    res.success = setSysCmd(sys_cmd);
    return true;
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "chassis_node");
    ros::NodeHandle n;
    ros::NodeHandle private_n("~");
    bool publish_tf;
    std::string odom_topic;
    std::string platform;
    float max_x_vel = 1.0;
    float max_z_omega = 1.0;

    private_n.param("publish_tf", publish_tf, false);
    private_n.param("odom_topic", odom_topic, std::string("/odom_chassis"));
    private_n.param("max_x_vel", max_x_vel, 3.0f);
    private_n.param("max_z_omega", max_z_omega, 3.0f);
    private_n.param("platform", platform, std::string(""));

    ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>(odom_topic, 1);
    ros::Publisher battery_pub =
            n.advertise<sensor_msgs::BatteryState>("/battery", 1);
    ros::Publisher io_state_pub =
            private_n.advertise<std_msgs::Int32>("io_state", 10);
    ros::Subscriber cmd_sub = n.subscribe("/cmd_vel", 1, cmdMsgCallback);
    ros::Subscriber motor_cmd_sub =
            private_n.subscribe("motor_off", 1, motorCmdCallback);
    ros::Subscriber io_sub = private_n.subscribe("io", 10, ioCallback);
    ros::Subscriber state_light_sub =
            private_n.subscribe("state_light", 1, stateLightCallback);
    ros::Subscriber info_sub = private_n.subscribe("info", 1, infoCallback);
    ros::ServiceServer server = private_n.advertiseService("charge", charge);

    std::string dev;
    if (!chassisInit(dev)) {
        ROS_ERROR("chassis init error");
        return 0;
    }
    ROS_INFO("chassis init with port %s", dev.c_str());

    char version[32] = {0};
    if (getVersion(version))
        ROS_INFO("chassis version: %s", version);

    int cnt = 0;
    bool first_odom = true;
    ros::Rate r(20.0);
    float pose[3] = {0};
    ros::Time current_time = ros::Time::now();
    tf::TransformBroadcaster odom_broadcaster;

    while (ros::ok()) {
        r.sleep();
        ros::spinOnce();
        current_time = ros::Time::now();

        if (current_time - cmd_update_time > ros::Duration(CMD_TIME_OUT / 1E3)) {
            aim_x_vel = 0.0f;
            aim_y_vel = 0.0f;
            aim_z_omega = 0.0f;
        }

        aim_x_vel = std::min(max_x_vel, std::max(-max_x_vel, aim_x_vel));
        aim_y_vel = std::min(max_x_vel, std::max(-max_x_vel, aim_y_vel));
        aim_z_omega = std::min(max_z_omega, std::max(-max_z_omega, aim_z_omega));
        ChassisData data = {aim_x_vel, aim_y_vel, aim_z_omega, light, 0};

        if (!updateChassisInfo(&data)) continue;

        io = data.io;
        sys_cmd = data.cmd;
        sensor_msgs::BatteryState state;
        if (cnt++ % 10 == 0) {
            state.header.stamp = current_time;
            state.voltage = data.voltage;
            state.charge = std::fmod(data.current, 100.0f);
            state.current = (data.current - state.charge) / 10000.0f;
            state.percentage = data.power;
            state.power_supply_status = data.state;
            battery_pub.publish(state);
        }

        std_msgs::Int32 io_state_msg;
        io_state_msg.data = data.io;
        io_state_pub.publish(io_state_msg);

        if (first_odom) {
            pose[0] = data.odom_x;
            pose[1] = data.odom_y;
            pose[2] = data.odom_z;
            first_odom = false;
        }

        geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(
            angles::normalize_angle(data.odom_z - pose[2]));
        geometry_msgs::TransformStamped odom_trans;
        odom_trans.header.stamp = current_time;
        odom_trans.header.frame_id = "odom";
        odom_trans.child_frame_id = "base_link";

        odom_trans.transform.translation.x =
                cos(-pose[2]) * (data.odom_x - pose[0]) -
                sin(-pose[2]) * (data.odom_y - pose[1]);
        odom_trans.transform.translation.y =
                sin(-pose[2]) * (data.odom_x - pose[0]) +
                cos(-pose[2]) * (data.odom_y - pose[1]);
        odom_trans.transform.translation.z = 0.0;
        odom_trans.transform.rotation = odom_quat;
        if (publish_tf) odom_broadcaster.sendTransform(odom_trans);

        nav_msgs::Odometry odom;
        odom.header.stamp = current_time;
        odom.header.frame_id = "odom";
        odom.pose.pose.position.x = odom_trans.transform.translation.x;
        odom.pose.pose.position.y = odom_trans.transform.translation.y;
        odom.pose.pose.position.z = 0.0;
        odom.pose.pose.orientation = odom_quat;
        odom.pose.covariance[0] = (SystemState::SYS_ESTOP == data.state) ? -1 : 1;
        odom.child_frame_id = "base_link";
        odom.twist.twist.linear.x = data.vel_x;
        odom.twist.twist.linear.y = 0;
        odom.twist.twist.angular.z = data.vel_z;
        odom_pub.publish(odom);

        if (cnt % 10 == 0)
            ROS_INFO_THROTTLE(2.0, "state: %.1f, %.1f, %d, %d, odom: %f, %f, err: %X",
                          data.voltage, data.current, data.power, data.state,
                          data.vel_x, data.vel_z, data.err);
        else
            ROS_INFO_THROTTLE(2.0, "odom: %f, %f", data.vel_x, data.vel_z);
    }
}
